#include <math.h>
#include <chassis_controller/kinematics.h>
#include <chassis_controller/kinematics_multi_pivots.h>
#include <chassis_controller/pid.h>
int getIndexOfSmallestElement(double arr[], int size)
{
  int smallestIndex = 0;
  for (int index = smallestIndex; index < size; index++)
  {
    if (fabs(arr[smallestIndex]) > fabs(arr[index]))
    {
      smallestIndex = index;
    }
  }
  return smallestIndex;
}

namespace chassis_controller
{

  MultiPivotsKinematics::MultiPivotsKinematics(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node) : 
      Kinematics(node),
      pose_x_(0),
      pose_y_(0),
      radius_(0.115),
      max_steer_(60),
      steer_tolerance_(0.087266463), // 5 degree
      steer_follow_tolerance_(1.0), // 60 degree
      MINIMUM_TRANSLATIONAL_VELOCITY(0.01),
      USE_ANGLE_TOLERANCE(false),
      USE_LIMITED_STEER_ANGLE(false),
      motor_rotational_velocity_(0),
      servo_steer_angle_(0),
      motor_direction_(0),
      desired_wheel_mount_velocity_x_(0),
      desired_wheel_mount_velocity_y_(0)

  {
  }
  void MultiPivotsKinematics::init(int pivot_num, pid_param steer_pid_param, SpeedLimiter limiter_liner, SpeedLimiter limiter_ang, SpeedLimiter limiter_steer) // PID instantiation
  {
    invert_flag_.resize(pivot_num);
    target_invert_flag_.resize(pivot_num);
    liner_setpoint_.resize(pivot_num);
    std::fill(liner_setpoint_.begin(), liner_setpoint_.end(), 0.0);
    angle_setpoint_.resize(pivot_num);
    std::fill(angle_setpoint_.begin(), angle_setpoint_.end(), 0.0);
    angle_setpoint_last_valid.resize(pivot_num);
    std::fill(angle_setpoint_last_valid.begin(), angle_setpoint_last_valid.end(), 0.0);
    limiter_liner_.resize(pivot_num);
    last0_cmd_liner_.resize(pivot_num);
    std::fill(last0_cmd_liner_.begin(), last0_cmd_liner_.end(), 0.0);
    last1_cmd_liner_.resize(pivot_num);
    std::fill(last1_cmd_liner_.begin(), last1_cmd_liner_.end(), 0.0);
    limiter_steer_.resize(pivot_num);
    last0_cmd_steer_.resize(pivot_num);
    std::fill(last0_cmd_steer_.begin(), last0_cmd_steer_.end(), 0.0);
    last1_cmd_steer_.resize(pivot_num);
    std::fill(last1_cmd_steer_.begin(), last1_cmd_steer_.end(), 0.0);
    reduce_factor_liner_.resize(pivot_num);
    std::fill(reduce_factor_liner_.begin(), reduce_factor_liner_.end(), 0.0);
    for (int i = 0; i < pivot_num; i++)
    {
      PID steer_pid(node_);
      steer_pid.pid_param_ = steer_pid_param;
      PID_vector_.push_back(steer_pid);
      limiter_liner_[i] = limiter_liner;
      limiter_steer_[i] = limiter_steer;
    }
    start_time_ = node_->now().seconds();
    steer_fall_time_ = node_->now().seconds();
    
    node_->get_parameter_or("/base/controllers/twist_controller/brake_time", brake_time_, brake_time_);
    DEBUG_OUT("[MultiPivotsKinematics] get brake time: %.4fs.", brake_time_);
    node_->get_parameter_or("/base/controllers/twist_controller/enable_brake_control", enable_brake_control_, false);
    DEBUG_OUT("[MultiPivotsKinematics] get enable_brake_control: %d.", enable_brake_control_);
    node_->get_parameter_or("/base/controllers/twist_controller/steer_delay", steer_delay_, 0.0);
    DEBUG_OUT("[ChassisController] get steer_delay_: %f.", steer_delay_);    
  }

  void MultiPivotsKinematics::updateParam(int pivot_num, pid_param steer_pid_param)
  {
    for (int i = 0; i < pivot_num; i++)
    {
      PID_vector_[i].pid_param_ = steer_pid_param;
    }
  }
  void MultiPivotsKinematics::getCommand(double vx, double vy, double va)
  {
    setpoint_.x = vx;
    setpoint_.y = vy;
    setpoint_.a = va;
    //DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiPivotsKinematics] current velocity setpoint :" << setpoint_.x << setpoint_.y << setpoint_.a);
    // DEBUG_STREAM_OUT("[MultiPivotsKinematics] current velocity setpoint :" << setpoint_.x << setpoint_.y << setpoint_.a);
  }

  void MultiPivotsKinematics::getVelocity(double vx, double vy, double va)
  {
    current_vel_.x = vx;
    current_vel_.y = vy;
    current_vel_.a = va;
    // DEBUG_OUT("current velocity :" << current_vel_.x << current_vel_.y << current_vel_.a);
  }

  void MultiPivotsKinematics::checkSteerDirection(double target, double actual, double max, double min, bool &positive_allowed, bool &negtive_allowed)
  {
    positive_allowed = true;
    negtive_allowed = true;
    if (actual > min && actual < max)
    {
      if (target > max)
        positive_allowed = false;
      if (target < min)
        negtive_allowed = false;
    }
    if (actual > max)
      positive_allowed = false;
    if (actual < min)
      negtive_allowed = false;
  }

  double MultiPivotsKinematics::normAngle2(double angle)
  {
    double fRad = angle;
    // Scale the angle into [-PI, +)
    while (fRad < -PI)
      fRad += 2 * PI;
    // Scale the angle into [-PI, PI)
    while (fRad >= PI)
      fRad -= 2 * PI;
    return fRad;
  }

  void MultiPivotsKinematics::update(std::vector<Spivot_value> &pivot_value,
                                     std::vector<Spivot_param> &pivot_param,
                                     std::vector<Spivot_setpoint> &pivot_setpoint, const rclcpp::Duration &period,
                                     bool ready_move)
  {
    pivots_num_ = pivot_value.size();
    steering_ = false;
    steer_follow_err_ = false;
    bool stop_req = false;
    drive_request_ = abs(setpoint_.x) > 0.00005 || abs(setpoint_.y) > 0.00005 || abs(setpoint_.a) > 0.00005;
    standstill_ = abs(current_vel_.x) < 0.005 && abs(current_vel_.y) < 0.005 && abs(current_vel_.a) < 0.005;

    if (drive_request_){
      no_drive_time_ = node_->now().seconds();
    }
    bool no_drive = (node_->now().seconds() -no_drive_time_) > 1.0;

    if (!drive_request_){
      movement_ = false;
    }
    drive_req_trigger_ = drive_request_ && !drive_request_old_;
    drive_request_old_ = drive_request_; 

    if (drive_request_)
    {
      steer_cmd_id_valid_ = steer_cmd_id_;
      start_time_ = node_->now().seconds();
    }

    if (enable_brake_control_) {
      double time_elapse = node_->now().seconds() - start_time_;
      if (time_elapse < brake_time_) {
        braker_open_ = true;
        DEBUG_OUT_THROTTLE(get_clock(), 
            500, "[MultiPivotsKinematics] brake release.");
      }
      else {
        braker_open_ = false;
        DEBUG_OUT_THROTTLE(get_clock(), 
            500, "[MultiPivotsKinematics] brake hold.");
        return;
      }

      if (!ready_move) {
        if (braker_open_)
          DEBUG_OUT_THROTTLE(get_clock(), 
              50, "[MultiPivotsKinematics] waiting for joints enable.");
        return;
      }
    }
    
    //RCLCPP_INFO_THROTTLE(0.1, "braker_open_flag %d" ,braker_open_) ;
    for (int i = 0; i < pivots_num_; i++)
    {
      // reset states
      pivot_value[i].steering = false;
      pivot_value[i].steer_follow_err = false;

      /* step1 calculate theoretical value */
      double temp_vx = setpoint_.x - setpoint_.a * pivot_param[i].offset_y;
      double temp_vy = setpoint_.y + setpoint_.a * pivot_param[i].offset_x;
      double temp_liner = sqrt(temp_vx * temp_vx + temp_vy * temp_vy);
      double temp_angle = 0.0;
      if (abs(temp_liner) > 0.0)
        temp_angle = atan2(temp_vy, temp_vx);
      // DEBUG_STREAM_OUT("[MultiKinematics] pivot " << i << "theoretical liner:" << temp_liner);
      // DEBUG_STREAM_OUT("[MultiKinematics] pivot " << i << "theoretical value vx,vy,liner,angle:" << temp_vx << "," << temp_vy << ","<< temp_liner <<"," << temp_angle);

      /*step2 estimate invert flag of pivot*/
      double temp_diff_angle, temp_diff_angle_alt, actual_angle, max_angle, min_angle;
      bool negtive_allowed, positive_allowed, forced_left, forced_right, suspend_invert,inverted=false;

      max_angle = pivot_param[i].max_angle;
      min_angle = pivot_param[i].min_angle;
      actual_angle = pivot_value[i].steer_position;
      forced_left = pivot_setpoint[i].invert;
      forced_right = pivot_setpoint[i].forbidden_invert;
      suspend_invert = pivot_setpoint[i].suspend_invert;

      if (!(movement_))
        temp_liner = 0.0;

      if (invert_flag_[i])
      {
        if (temp_angle >= 0.0)
          temp_angle = temp_angle - PI;
        else
          temp_angle = temp_angle + PI;

        temp_liner *= -1.0;
        //DEBUG_STREAM_OUT_THROTTLE(0.5, "kinematics pivot " << i <<"inverted");
        // DEBUG_STREAM_OUT("[MultiKinematics] pivot " << i <<"inverted");
      }
      // DEBUG_STREAM_OUT("[MultiKinematics] temp_angle after invert " << temp_angle);

      // if (!movement_){
      //   DEBUG_STREAM_OUT_THROTTLE(0.5,"kinematics pivot " << i <<"inverted");
      // }        
      pivot_value[i].inverted = invert_flag_[i];
      // DEBUG_OUT("kinematics pivot " << i << "invert flag:" << pivot_value[i].inverted);

      if (!movement_ && drive_request_ && forced_left && temp_angle <= 0) // 强制左转
      {
        target_invert_flag_[i] = !invert_flag_[i];
        inverted = true;
        //DEBUG_STREAM_OUT_THROTTLE(0.5,"pivot:" << i <<"steer left intentionally");
        // DEBUG_STREAM_OUT("[MultiKinematics] pivot:" << i <<"steer left intentionally");
      }
      else if (!movement_ && drive_request_ && forced_right && temp_angle >= 0) // 强制右转
      {
        target_invert_flag_[i] = !invert_flag_[i];
        inverted = true;
        //DEBUG_STREAM_OUT_THROTTLE(0.5,"pivot:" << i <<"steer right intentionally");
        // DEBUG_STREAM_OUT("[MultiKinematics] pivot:" << i <<"steer right intentionally");
      }

      // 反转条件2 : 超出限位逻辑处理
      if (!movement_)
      {
        // 如果原地打舵(未运行) 到达限位自动反转
        if((temp_angle > max_angle) && forced_left){
          temp_angle = pivot_param[i].max_angle;
          inverted = true;
        }
        else if((temp_angle < min_angle) && forced_right){
          temp_angle = pivot_param[i].min_angle;
          inverted = true;
        }
        else if ((temp_angle > max_angle || temp_angle < min_angle) && !inverted)
        {
          target_invert_flag_[i] = !invert_flag_[i];
          inverted = true;
          //DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"out of steer range");
          //DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"temp_angle is"<< temp_angle);
          // DEBUG_STREAM_OUT("[MultiKinematics] pivot:" << i <<"out of steer range");
          // DEBUG_STREAM_OUT("[MultiKinematics] pivot:" << i <<"temp_angle is"<< temp_angle);
        }       
      }
      else
      {
        // 运行起来 如果禁止反向 打到极限位值
        if (suspend_invert && temp_angle > max_angle)
        {
          temp_angle = pivot_param[i].max_angle;
          // DEBUG_STREAM_OUT("pivot:" << i <<"suspend_invert and temp_angle is greater than max_angle");
          // DEBUG_STREAM_OUT("pivot:" << i <<"temp_angle is"<< temp_angle);
        }
        else if (suspend_invert && temp_angle < min_angle)
        {
          temp_angle = pivot_param[i].min_angle;
          // DEBUG_STREAM_OUT("pivot:" << i <<"suspend_invert and temp_angle is less than min_angle");
          // DEBUG_STREAM_OUT("pivot:" << i <<"temp_angle is"<< temp_angle);          
        }
      }

      // 反转条件3: 计算最近路径
      temp_diff_angle = temp_angle - actual_angle;
      // DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<" temp_angle:"<<temp_angle<<"actual angle:"<<actual_angle<< " temp_diff_angle:"<<temp_diff_angle);
      // DEBUG_STREAM_OUT("[MultiKinematics] pivot:" << i <<" temp_angle:"<<temp_angle<<"actual angle:"<<actual_angle<< " temp_diff_angle:"<<temp_diff_angle);
      checkSteerDirection(temp_angle, actual_angle, max_angle, min_angle, positive_allowed, negtive_allowed);
      //  DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"first checkSteerDirection,positive allowed:"<<positive_allowed<<"negative allowed:"<<negtive_allowed);
      
      if (!movement_ && drive_req_trigger_ && !forced_left && !forced_right && !inverted)
          // (target_invert_flag_[i] == invert_flag_[i]))
      {
        // calculate diff angle
        if (temp_diff_angle >= 0.0)
        {
          if (temp_diff_angle > PI && negtive_allowed || temp_diff_angle <= PI && negtive_allowed && !positive_allowed){
            //  DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"first diff angle positive:"<<temp_diff_angle);
            temp_diff_angle = 2 * PI - temp_diff_angle;
            //  DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"first diff angle positive norm:"<<temp_diff_angle);
          }
        }
        else
        {
          if (temp_diff_angle < -PI && positive_allowed || temp_diff_angle >= -PI && !negtive_allowed && positive_allowed){
            //  DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"first diff angle negative:"<<temp_diff_angle);
            temp_diff_angle = 2 * PI + temp_diff_angle;
            //  DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"first diff angle negative norm:"<<temp_diff_angle);
          }
        }

        // calculate alternative diff angle
        temp_diff_angle_alt = normAngle2(temp_angle + PI - actual_angle);
        double temp_angle_alt = normAngle2(temp_angle + PI );
        // DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"temp_angle:"<<temp_angle<<"actual angle:"<<actual_angle<< "temp_diff_angle_alt:"<<temp_diff_angle_alt);
        checkSteerDirection(temp_angle_alt, actual_angle, max_angle, min_angle, positive_allowed, negtive_allowed);
        // DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"second checkSteerDirection ,positive allowed:"<<positive_allowed<<"negative allowed:"<<negtive_allowed);
        if (temp_diff_angle_alt >= 0.0)
        {
          if (temp_diff_angle_alt > PI && negtive_allowed || temp_diff_angle_alt <= PI && negtive_allowed && !positive_allowed){
            // DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"second diff angle positive:"<<temp_diff_angle_alt);
            temp_diff_angle_alt = 2 * PI - temp_diff_angle_alt;
            // DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"second diff angle positive norm:"<<temp_diff_angle_alt);
          }
        }
        else
        {
          if (temp_diff_angle_alt < -PI && positive_allowed || temp_diff_angle_alt >= -PI && !negtive_allowed && positive_allowed){
            // DEBUG_STREAM_OUT("[MultiKinematics] pivot:" << i <<"second diff angle negative:"<<temp_diff_angle_alt);
            temp_diff_angle_alt = 2 * PI + temp_diff_angle_alt;
            // DEBUG_STREAM_OUT("[MultiKinematics] pivot:" << i <<"second diff angle negative norm:"<<temp_diff_angle_alt);
          }
        }
        DEBUG_OUT_THROTTLE(get_clock(), 
            500,"[MultiKinematics] pivot:" << i <<"first diff angle is :" << temp_diff_angle << ","
                                                << "second diff angle is :" << temp_diff_angle_alt);
        // Take the shorter way
        if (abs(temp_diff_angle) > PI_2 && abs(temp_diff_angle_alt) < abs(temp_diff_angle) /* && !pivot_value[i].inverted */
            && !(normAngle2(temp_angle + PI) > max_angle || normAngle2(temp_angle + PI) < min_angle))
        {
          target_invert_flag_[i] = !target_invert_flag_[i];
          // DEBUG_OUT_THROTTLE(0.5,"[MultiKinematics] pivot:" << i <<"take the second angle");
        }

      }

      /*step3 state transition diagram*/
      temp_diff_angle = abs(temp_angle - actual_angle);
      if (temp_diff_angle > PI)
        temp_diff_angle = abs(2 * PI - temp_diff_angle);
      // DEBUG_STREAM_OUT("absolute diff angle is : " << temp_diff_angle << ",temp_angle=" << temp_angle << ",actual_angle=" << actual_angle);
      // RCLCPP_INFO_THROTTLE(0.1, "absolute diff angle is : %.6f  temp_angle: %.6f  actual_angle: %.6f", temp_diff_angle, temp_angle, actual_angle);

      // steer outoff tolerance in start stage
      if (temp_diff_angle > steer_tolerance_ && !movement_ && drive_request_)
      {
        pivot_value[i].steering = true;
        steering_ = true;
        //DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot["<<i<<"] steering flag : " << steering_);
        // DEBUG_STREAM_OUT("[MultiKinematics] pivot["<<i<<"] steering flag : " << steering_);
      }

      // steer exceed tolerance in moving state
      if (temp_diff_angle > steer_follow_tolerance_ && movement_)
      {
        pivot_value[i].steer_follow_err = true;
        steer_follow_err_ = true;
      }
      liner_setpoint_[i] = temp_liner;
      angle_setpoint_[i] = temp_angle;

      
      const double cmd_dt(period.seconds());
      //计算每个轴的速度缩放比
      double temp_liner_reduce = temp_liner;
      limiter_liner_[i].max_velocity = abs(setpoint_.x) > abs(setpoint_.y) ? abs(setpoint_.x) : abs(setpoint_.y);
      limiter_liner_[i].min_velocity = -limiter_liner_[i].max_velocity;
      //RCLCPP_INFO_THROTTLE(0.1 ,"pivot %d  max_velocity: %.4f  min_velocity: %.4f ", i, limiter_liner_[i].max_velocity , limiter_liner_[i].min_velocity);
      //DEBUG_STREAM_OUT("pivot "<< i << "max_velocity " << limiter_liner_[i].max_velocity );
      //DEBUG_STREAM_OUT("pivot "<< i << "min_velocity " << limiter_liner_[i].min_velocity );
      reduce_factor_liner_[i] = limiter_liner_[i].limit(temp_liner_reduce, last0_cmd_liner_[i], last1_cmd_liner_[i], cmd_dt);
      last1_cmd_liner_[i] = last0_cmd_liner_[i];
      last0_cmd_liner_[i] = temp_liner_reduce;
      if (setpoint_.x <0.01 && setpoint_.y <0.01)
        reduce_factor_liner_[i] = 1.0;
      //DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot "<< i << "reduce_factor_liner " << reduce_factor_liner_[i]);
      // DEBUG_STREAM_OUT("[MultiKinematics] pivot "<< i << "reduce_factor_liner " << reduce_factor_liner_[i]);
    };
    // 对每个驱动的线速度进行限制，选择所有驱动中最小的缩放比
    double reduce_factor_min = 1.0;
    for (int i = 0; i < pivots_num_; i++)
    {
      if (reduce_factor_liner_[i] < reduce_factor_min )
      reduce_factor_min = reduce_factor_liner_[i];          
    }
    //DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] reduce_factor_min " << reduce_factor_min);
    // DEBUG_STREAM_OUT("[MultiKinematics] reduce_factor_min " << reduce_factor_min);
    for (int i = 0; i < pivots_num_; i++)
    {
      liner_setpoint_[i] *= reduce_factor_min;
      //DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] pivot "<< i << "reduced liner_setpoint_ " << liner_setpoint_[i]);
      // DEBUG_STREAM_OUT("[MultiKinematics] pivot "<< i << "reduced liner_setpoint_ " << liner_setpoint_[i]);
    }
    
    stop_req = steering_ || steer_follow_err_;
    // DEBUG_OUT("reqStop flag : " << stop_req);

    // use last valid value while inverting
    if (target_invert_flag_ != invert_flag_)
    {
      stop_req = true;
      for (int i = 0; i < pivots_num_; i++)
      {
        angle_setpoint_[i] = angle_setpoint_last_valid[i];
      }
      // DEBUG_STREAM_OUT("[MultiKinematics] use last vaild angle while inverting");
    }

    else if (drive_request_ && movement_) // save last valid value
    {
      // DEBUG_STREAM_OUT("[MultiKinematics] save last vaild angle ");
      for (int i = 0; i < pivots_num_; i++)
      {
        angle_setpoint_last_valid[i] = angle_setpoint_[i];        
        // DEBUG_STREAM_OUT("[MultiKinematics] pivot["<<i<<"] angle_setpoint_last_valid "<<angle_setpoint_last_valid[i]);
      }
    }
    // keep angle
    if (!drive_request_)
    {    
      for (int i = 0; i < pivots_num_; i++)
      {
        angle_setpoint_[i] = angle_setpoint_last_valid[i];
        target_invert_flag_[i] = false;
        invert_flag_[i] = false; 
      }
    }

    if (target_invert_flag_ != invert_flag_ && standstill_)
    {
      invert_flag_ = target_invert_flag_;
      // DEBUG_STREAM_OUT("[MultiKinematics] assign invert_flag while standdtill");
    }

    if (!movement_ && drive_request_ && !stop_req)
      movement_ = true;

    /*  steer angle control */
    for (int i = 0; i < pivots_num_; i++)
    {
      //DEBUG_STREAM_OUT_THROTTLE(0.5,"[MultiKinematics] [steerWheel]: pivot["<<i<<"]steer_setpoint=" << angle_setpoint_[i] << ",velocity_setpoint=" << liner_setpoint_[i]);
      // DEBUG_STREAM_OUT("[MultiKinematics] [steerWheel]: pivot["<<i<<"]steer_setpoint=" << angle_setpoint_[i] << ",velocity_setpoint=" << liner_setpoint_[i]);
      if (pivot_param[i].pivot_type == STEERWHEEL) // assign angle and velocity if active pivot
      {
        pivot_setpoint[i].steer_setpoint = angle_setpoint_[i];
        pivot_setpoint[i].velocity_setpoint = liner_setpoint_[i];
        if (pivot_param[i].joint_param[0].drive_mode == TRACTION)
          pivot_setpoint[i].joint_setpoint[0].velocity_setpoint = pivot_setpoint[i].velocity_setpoint;
        else if (pivot_param[i].joint_param[0].drive_mode == STEER)
          pivot_setpoint[i].joint_setpoint[0].position_setpoint = pivot_setpoint[i].steer_setpoint;

        if (pivot_param[i].joint_param[1].drive_mode == TRACTION)
          pivot_setpoint[i].joint_setpoint[1].velocity_setpoint = pivot_setpoint[i].velocity_setpoint;
        else if (pivot_param[i].joint_param[1].drive_mode == STEER)
          pivot_setpoint[i].joint_setpoint[1].position_setpoint = pivot_setpoint[i].steer_setpoint;
      }
      else if (pivot_param[i].pivot_type == DIFFWHEEL) // calculate velocity of two wheels if passive pivot
      {

        /* step1 calculate position loop */
        PID_vector_[i].pid_param_.period = period.seconds();

        double delta_omiga = PID_vector_[i].positionPID(pivot_value[i].steer_position, angle_setpoint_[i], false, no_drive);

        DEBUG_OUT_THROTTLE(get_clock(), 
            500,"[MultiKinematics] steering pivot " << i << "PID setpoint"<< angle_setpoint_[i] << " ,feedback :" << pivot_value[i].steer_position << " ,PID output :" << delta_omiga);
        // limit steer acc
        if (steering_){
          //DEBUG_STREAM_OUT("[MultiKinematics] steering PID output before limiter: " << delta_omiga);
        }
        const double cmd_dt(period.seconds());
        double reduce_factor_steer;
        reduce_factor_steer = limiter_steer_[i].limit(delta_omiga, last0_cmd_steer_[i], last1_cmd_steer_[i], cmd_dt);
        last1_cmd_steer_[i] = last0_cmd_steer_[i];
        last0_cmd_steer_[i] = delta_omiga;
        DEBUG_OUT_THROTTLE(get_clock(), 
            500,"[MultiKinematics] steering PID output after limiter: " << delta_omiga);
        // 需要限制线速度的输出，转舵为0，限制加速度
        if (!steering_)
        {
          pivot_setpoint[i].joint_setpoint[0].velocity_setpoint = liner_setpoint_[i] - (setpoint_.a + delta_omiga) * pivot_param[i].radius_ringmount;
          pivot_setpoint[i].joint_setpoint[1].velocity_setpoint = liner_setpoint_[i] + (setpoint_.a + delta_omiga) * pivot_param[i].radius_ringmount;
          // DEBUG_OUT("liner_setpoint_ :" << liner_setpoint_[i]);
          DEBUG_OUT_THROTTLE(get_clock(), 
              500,"[MultiKinematics] not steering pivot " << i << "joint 0 velocity setpoint:" << pivot_setpoint[i].joint_setpoint[0].velocity_setpoint);
          DEBUG_OUT_THROTTLE(get_clock(),
              500,"[MultiKinematics] not steering pivot " << i << "joint 1 velocity setpoint:" << pivot_setpoint[i].joint_setpoint[1].velocity_setpoint);
        }
        else
        {
          pivot_setpoint[i].joint_setpoint[0].velocity_setpoint = -delta_omiga * pivot_param[i].radius_ringmount;
          pivot_setpoint[i].joint_setpoint[1].velocity_setpoint = delta_omiga * pivot_param[i].radius_ringmount;
          // DEBUG_OUT("liner_setpoint_ :" << liner_setpoint_[i]);
          
          // DEBUG_STREAM_OUT("[MultiKinematics] steering pivot " << i << "joint 0 velocity setpoint:" << pivot_setpoint[i].joint_setpoint[0].velocity_setpoint);
          // DEBUG_STREAM_OUT("[MultiKinematics] steering pivot " << i << "joint 1 velocity setpoint:" << pivot_setpoint[i].joint_setpoint[1].velocity_setpoint);
        }
        if (!drive_request_)
        {
          pivot_setpoint[i].joint_setpoint[0].velocity_setpoint = 0.0;
          pivot_setpoint[i].joint_setpoint[1].velocity_setpoint = 0.0;
        }
        // DEBUG_OUT("pivot " << i << "joint 0 velocity setpoint:" << pivot_setpoint[i].joint_setpoint[0].velocity_setpoint);
        // DEBUG_OUT("pivot " << i << "joint 1 velocity setpoint:" << pivot_setpoint[i].joint_setpoint[1].velocity_setpoint);
        // RCLCPP_INFO_THROTTLE(0.1, "pivot[%d] joint[0] velocity_setpoint:: %.6f  joint[1] velocity_setpoint: %.6f", i, 
        //   pivot_setpoint[i].joint_setpoint[0].velocity_setpoint, pivot_setpoint[i].joint_setpoint[1].velocity_setpoint);
       
      }
    }
  
    if(!steering_ && steering_last_){
      steer_fall_time_ = node_->now().seconds();
    }
    if(!steering_ && (node_->now().seconds() - steer_fall_time_) > steer_delay_){
      steering_ = false ;
    }

    steering_last_ = steering_;
  
  }

  void MultiPivotsKinematics::setCommand(std::vector<Spivot_value> &pivot_value,
                                         std::vector<Spivot_param> &pivot_param,
                                         std::vector<Spivot_setpoint> &pivot_setpoint,
                                         const rclcpp::Duration &period)
  {
    for (int i = 0; i < pivots_num_; i++)
    {
      assert(pivot_param[i].driving_wheel_diameter > 0.01);
      double MperSec2RadPerSec = 2.0 / pivot_param[i].driving_wheel_diameter;
      for (int j = 0; j < 2; ++j)
      {
        std::string pivot_joint_name = pivot_param[i].joint_param[j].name;
        assert (!pivot_joint_name.empty());
      
        if (pivot_param[i].joint_param[j].drive_mode == TRACTION) // assign angle and velocity if active pivot
        { 
          double temp_velocity = 0.0;
          temp_velocity = pivot_setpoint[i].joint_setpoint[j].velocity_setpoint * MperSec2RadPerSec;
          pivot_param[i].joint_param[j].handle.joint_command_interfaces[0].get().set_value(temp_velocity); // rad/s
          DEBUG_OUT_THROTTLE(get_clock(), 
              500,"[MultiKinematics] joint["<<pivot_joint_name<<"] velocity setpoint is:"<<temp_velocity);
        }
        else if (pivot_param[i].joint_param[j].drive_mode == STEER)
        {
          double temp_position;
          temp_position = pivot_setpoint[i].joint_setpoint[j].position_setpoint + pivot_param[i].steer_offset;
          pivot_param[i].joint_param[j].handle.joint_command_interfaces[1].get().set_value(temp_position);
          DEBUG_OUT_THROTTLE(get_clock(), 
              500,"[MultiKinematics] joint["<<pivot_joint_name<<"] position setpoint is:"<<temp_position);
        }
      }
    }

    if (pivots_num_ == 4) {
      // RCLCPP_INFO_THROTTLE(0.2, "\npivot[0] joint[0]  vel: %.6f  joint[1]  vel: %.6f\npivot[1] joint[0]  vel: %.6f  joint[1]  vel: %.6f\npivot[2] joint[0]  vel: %.6f  joint[1]  vel: %.6f\npivot[3] joint[0]  vel: %.6f  joint[1]  vel: %.6f",
      //             pivot_setpoint[0].joint_setpoint[0].velocity_setpoint * 12.5,
      //             pivot_setpoint[0].joint_setpoint[1].velocity_setpoint * 12.5,
      //             pivot_setpoint[1].joint_setpoint[0].velocity_setpoint * 12.5,
      //             pivot_setpoint[1].joint_setpoint[1].velocity_setpoint * 12.5,
      //             pivot_setpoint[2].joint_setpoint[0].velocity_setpoint * 12.5,
      //             pivot_setpoint[2].joint_setpoint[1].velocity_setpoint * 12.5,
      //             pivot_setpoint[3].joint_setpoint[0].velocity_setpoint * 12.5,
      //             pivot_setpoint[3].joint_setpoint[1].velocity_setpoint * 12.5);
    }
  }

  void MultiPivotsKinematics::setSteerCmdId(uint64_t steer_cmd_id)
  {
    steer_cmd_id_ = steer_cmd_id;
  }

  bool MultiPivotsKinematics::getSteeringFlag()
  {
    return steering_;
  }

  bool MultiPivotsKinematics::getSteerErrFlag()
  {
    return steer_follow_err_;
  }

  bool MultiPivotsKinematics::getBrakerFlag()
  {
    return braker_open_;
  }

  uint64_t MultiPivotsKinematics::getSteerCmdId()
  {
    return steer_cmd_id_valid_;
  }

}
